const math = require('mathjs');
const proj4 = require('proj4');

// WGS84坐标系的参数
const a = 6378137.0;  // 地球半长轴，单位：米
const f = 1 / 298.257223563;  // 扁率
const b = a * (1 - f);  // 地球半短轴，单位：米

// 将经纬度、高度（LLH）转换为XYZ坐标（地心地固坐标系）
function llh2xyz(llh) {
    const lat = llh[0];
    const lon = llh[1];
    const h = llh[2];

    const sinLat = Math.sin(lat);
    const cosLat = Math.cos(lat);
    const sinLon = Math.sin(lon);
    const cosLon = Math.cos(lon);

    const e2 = 2 * f - f * f;
    const N = a / Math.sqrt(1 - e2 * sinLat * sinLat);

    const x = (N + h) * cosLat * cosLon;
    const y = (N + h) * cosLat * sinLon;
    const z = ((1 - e2) * N + h) * sinLat;

    return [x, y, z];
}

// 将XYZ坐标转换为经纬度高度（LLH）
function xyz2llh(xyz) {
    const x = xyz[0];
    const y = xyz[1];
    const z = xyz[2];

    const lon = Math.atan2(y, x);
    const p = Math.sqrt(x * x + y * y);
    const lat = Math.atan2(z, p * (1 - f));
    const h = p / Math.cos(lat) - a / Math.sqrt(1 - f * (2 - f) * Math.sin(lat) * Math.sin(lat));

    return [lat, lon, h];
}

// 将XYZ坐标转换为ENU坐标系
function xyz2enu(xyz, orgxyz) {
    const [x, y, z] = xyz;
    const [orgX, orgY, orgZ] = orgxyz;

    const dX = x - orgX;
    const dY = y - orgY;
    const dZ = z - orgZ;

    const orgLLH = xyz2llh(orgxyz);
    const phi = orgLLH[0];
    const lambda = orgLLH[1];

    const sinPhi = Math.sin(phi);
    const cosPhi = Math.cos(phi);
    const sinLambda = Math.sin(lambda);
    const cosLambda = Math.cos(lambda);

    const R = [
        [-sinLambda, cosLambda, 0],
        [-sinPhi * cosLambda, -sinPhi * sinLambda, cosPhi],
        [cosPhi * cosLambda, cosPhi * sinLambda, sinPhi]
    ];

    const enu = math.multiply(R, [dX, dY, dZ]);

    return enu;
}

// 将ENU坐标转换为XYZ坐标系
function enu2xyz(enu, orgxyz) {
    const [e, n, u] = enu;
    const [orgX, orgY, orgZ] = orgxyz;

    const orgLLH = xyz2llh(orgxyz);
    const phi = orgLLH[0];
    const lambda = orgLLH[1];

    const sinPhi = Math.sin(phi);
    const cosPhi = Math.cos(phi);
    const sinLambda = Math.sin(lambda);
    const cosLambda = Math.cos(lambda);

    const R_inv = [
        [-sinLambda, -sinPhi * cosLambda, cosPhi * cosLambda],
        [cosLambda, -sinPhi * sinLambda, cosPhi * sinLambda],
        [0, cosPhi, sinPhi]
    ];

    const diffXYZ = math.multiply(R_inv, [e, n, u]);

    const x = diffXYZ[0] + orgX;
    const y = diffXYZ[1] + orgY;
    const z = diffXYZ[2] + orgZ;

    return [x, y, z];
}

// 计算四个顶点的LLH坐标
function calVertices(P_A_llh, P_B_llh, HEI, WID) {
    // 初始点P_1, P_2, P_3, P_4在L系中的坐标
    const P_1_L = [0, -WID / 2, -HEI / 2];
    const P_2_L = [0, -WID / 2, HEI / 2];
    const P_3_L = [0, WID / 2, -HEI / 2];
    const P_4_L = [0, WID / 2, HEI / 2];

    const P_A_xyz = llh2xyz(P_A_llh);
    const P_B_xyz = llh2xyz(P_B_llh);

    const AB_enu = xyz2enu(P_B_xyz, P_A_xyz);

    // 计算航向角heading和俯仰角pitch
    let heading;
    if (AB_enu[0] >= 0 && AB_enu[1] > 0) heading = Math.atan(AB_enu[0] / AB_enu[1]);
    if (AB_enu[0] > 0 && AB_enu[1] === 0) heading = Math.PI / 2;
    if (AB_enu[0] >= 0 && AB_enu[1] < 0) heading = Math.PI - Math.atan(-AB_enu[0] / AB_enu[1]);
    if (AB_enu[0] < 0 && AB_enu[1] < 0) heading = Math.PI + Math.atan(AB_enu[0] / AB_enu[1]);
    if (AB_enu[0] < 0 && AB_enu[1] === 0) heading = Math.PI + Math.PI / 2;
    if (AB_enu[0] < 0 && AB_enu[1] > 0) heading = 2 * Math.PI - Math.atan(-AB_enu[0] / AB_enu[1]);

    const pitch = Math.atan(AB_enu[2] / Math.sqrt(AB_enu[0] * AB_enu[0] + AB_enu[1] * AB_enu[1]));

    const roll = 0; // 假设滚转角为0

    // 计算DCM（方向余弦矩阵）
    const C_b_n = Euler_to_DCM(roll, pitch, heading);
    const C_ned_enu = [
        [0, 1, 0],
        [1, 0, 0],
        [0, 0, -1]
    ];

    const P_1_enu = math.multiply(C_ned_enu, math.multiply(C_b_n, P_1_L));
    const P_2_enu = math.multiply(C_ned_enu, math.multiply(C_b_n, P_2_L));
    const P_3_enu = math.multiply(C_ned_enu, math.multiply(C_b_n, P_3_L));
    const P_4_enu = math.multiply(C_ned_enu, math.multiply(C_b_n, P_4_L));

    const P_1_llh = xyz2llh(enu2xyz(P_1_enu, P_A_xyz));
    const P_2_llh = xyz2llh(enu2xyz(P_2_enu, P_A_xyz));
    const P_3_llh = xyz2llh(enu2xyz(P_3_enu, P_A_xyz));
    const P_4_llh = xyz2llh(enu2xyz(P_4_enu, P_A_xyz));

    return [P_1_llh, P_2_llh, P_3_llh, P_4_llh];
}

// 欧拉角转方向余弦矩阵 (DCM)
function Euler_to_DCM(roll, pitch, heading) {
    return [
        [Math.cos(pitch) * Math.cos(heading), -Math.cos(roll) * Math.sin(heading) + Math.sin(roll) * Math.sin(pitch) * Math.cos(heading), Math.sin(roll) * Math.sin(heading) + Math.cos(roll) * Math.sin(pitch) * Math.cos(heading)], [Math.cos(pitch) * Math.sin(heading), Math.cos(roll) * Math.cos(heading) + Math.sin(roll) * Math.sin(pitch) * Math.sin(heading), -Math.sin(roll) * Math.cos(heading) + Math.cos(roll) * Math.sin(pitch) * Math.sin(heading)], [-Math.sin(pitch), Math.sin(roll) * Math.cos(pitch), Math.cos(roll) * Math.cos(pitch)]];
}

module.exports = { calVertices };
